﻿using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

namespace Assignment_2
{
    /// <summary>
    /// This is a game component that implements IUpdateable.
    /// </summary>
    public class Robot : GameObject
    {
        #region Variables
        private Model _model;
        private Matrix realWorld;
        private Dictionary<String, Matrix> origTransforms = new Dictionary<string, Matrix>();
        int _slices = 0;
        int _stacks = 0;
        private SphereStuff _wf;
        public float leftRightRotation = 0.0f;
        float legRot = 0.0f;
        bool legDirection = false;
        float clavRot = 0.0f;
        bool clavDirection = false;
        float uArmRot = 0.0f;
        bool uArmDirection = false;
        float lArmRot = 0.0f;
        bool lArmDirection = false;
        float torsoRot = 0.0f;
        private float angle;
        private Vector2 unitVector;
        private Vector2 pos2d;

        #endregion

        #region Constants
        private const float legMax = 50.0f;
        private const float legMin = -50.0f;
        private const float clavMax = 10.0f;
        private const float clavMin = -10.0f;
        private const float uarmMax = 20.0f;
        private const float uarmMin = -20.0f;
        private const float larmMax = 90.0f;
        private const float larmMin = 70.0f;
        private const float torsoMax = 30.0f;
        private const float torsoMin = -30.0f;

        private const int MAXX = 97;
        private const int MINX = 3;
        private const int MAXY = 97;
        private const int MINY = 3;
        private const float rotationSpeed = 10.0f;
        private const float movementSpeed = 15.0f;

        private const float MOVEMENT_FORWARD_SPEED = 50.0f;
        private const float MOVEMENT_BACKWARD_SPEED = 35.0f;
        private const float TURN_SPEED = 0.01f;
        #endregion

        public Robot(GraphicsDevice graphicsDevice, SphereStuff wf, Model model, Vector3 initialPosition, int slices, int stacks, Matrix worldMatrix)
            : base(graphicsDevice, worldMatrix)
        {
            this._model = model;
            this._wf = wf;
            this._position = initialPosition;
            this._slices = slices;
            this._stacks = stacks;
            this.realWorld = worldMatrix;
            this.angle = 0f;
            this.unitVector = new Vector2(1, 0);
            this.pos2d = new Vector2(0, 0);
            Initialize();
        }

        /// <summary>
        /// Allows the game component to perform any initialization it needs to before starting
        /// to run.  This is where it can query for any required services and load content.
        /// </summary>
        public void Initialize()
        {
            this._model.Root.Transform = Matrix.CreateScale(0.05f)
                * Matrix.CreateRotationZ(MathHelper.ToRadians(90))
                * Matrix.CreateRotationX(MathHelper.ToRadians(-90))
                * Matrix.CreateTranslation(this.Position);
            foreach (ModelBone mb in _model.Bones) { origTransforms.Add(mb.Name, mb.Transform); }
        }

        public void Turn(Constants.Direction dir, GameTime gameTime)
        {
            if (dir == Constants.Direction.Left)
            {
                if (torsoRot <= torsoMax)
                {
                    torsoRot += 5.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
                }

                //    if (leftRightRotation < 0) leftRightRotation = 0;


                leftRightRotation += rotationSpeed * (float)gameTime.ElapsedGameTime.TotalSeconds;

                //                unitVector = Vector2.Transform(unitVector, Quaternion.CreateFromAxisAngle(Vector3.Up, leftRightRotation));

                //  this._rotation = Quaternion.CreateFromAxisAngle(Vector3.Up, leftRightRotation);

            }
            else if (dir == Constants.Direction.Right)
            {
                if (torsoRot >= torsoMin)
                {
                    torsoRot -= 5.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
                }

                //  if (leftRightRotation > 0) leftRightRotation = 0;
                leftRightRotation -= rotationSpeed * (float)gameTime.ElapsedGameTime.TotalSeconds;

                // unitVector = Vector2.Transform(unitVector, Quaternion.CreateFromAxisAngle(Vector3.Up, leftRightRotation));

                //    this._rotation = Quaternion.CreateFromAxisAngle(Vector3.Up, leftRightRotation);

            }

            AnimateLegs(gameTime, TURN_SPEED);
            //this._rotation = Quaternion.CreateFromAxisAngle(Vector3.Up, leftRightRotation); //new Quaternion(Vector3.Up, leftRightRotation);
        }

        public void Move(Constants.Direction dir, GameTime gameTime)
        {
            float timeDiff = (float)gameTime.ElapsedGameTime.TotalSeconds;

            if (dir == Constants.Direction.Forward)
            {

                AnimateLegs(gameTime, MOVEMENT_FORWARD_SPEED);
                AnimateArms(gameTime);
                //this.objWorld *= Matrix.CreateTranslation(0, 0, -0.1f);
                ////this.objWorld *= Matrix.CreateTranslation(0, 0, -MOVEMENT_FORWARD_SPEED);                
                //this.objWorld = Matrix.CreateTranslation(this._position);

                //unitvector 1,0
                //float angle
                //rotera unitvector utifrån angle

                //this.2d_position += unitvector.

                //this._position = wf.getspherecoords()

                pos2d += unitVector;

                /*
                this._position = new Vector3(100, 1.03f, 0); 
                this._model.Root.Transform = Matrix.CreateScale(0.05f)
                * Matrix.CreateRotationZ(MathHelper.ToRadians(90))
                * Matrix.CreateRotationX(MathHelper.ToRadians(-90))
                * Matrix.CreateTranslation(this.Position);
                 */

                //Forward();
            }

            else if (dir == Constants.Direction.Backward)
            {
                //animate backward
                AnimateLegs(gameTime, MOVEMENT_BACKWARD_SPEED);
                AnimateArms(gameTime);
                //this.objWorld *= Matrix.CreateTranslation(0, 0, 0.1f);
                pos2d -= unitVector;

                //Backward();
            }

            if (dir == Constants.Direction.Left)
            {
                this.Turn(Constants.Direction.Left, gameTime);
                //animate backward
                AnimateLegs(gameTime, TURN_SPEED);
                AnimateArms(gameTime);
                //this.objWorld *= Matrix.CreateTranslation(-0.1f, 0, 0);
                //this.objWorld *= Matrix.CreateTranslation(-TURN_SPEED, 0, 0);
                //this._position += new Vector3(-TURN_SPEED, 0, 0);
                //Left();
            }

            else if (dir == Constants.Direction.Right)
            {

                this.Turn(Constants.Direction.Right, gameTime);
                //animate backward
                AnimateLegs(gameTime, TURN_SPEED);
                AnimateArms(gameTime);
                //this.objWorld *= Matrix.CreateTranslation(0.1f, 0, 0);
                // this.objWorld *= Matrix.CreateTranslation(TURN_SPEED, 0, 0);
                //this._position += new Vector3(TURN_SPEED, 0, 0);
                // Right();
            }
        }

        private void AnimateArms(GameTime gameTime)
        {
            #region Clavicle
            if (clavRot >= clavMax) { clavDirection = false; }
            else if (clavRot <= clavMin) { clavDirection = true; }
            if (clavDirection)
            {
                clavRot += 3.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
            else
            {
                clavRot -= 3.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
            #endregion

            #region Upper arm
            if (uArmRot >= uarmMax) { uArmDirection = false; }
            else if (uArmRot <= uarmMin) { uArmDirection = true; }
            if (uArmDirection)
            {
                uArmRot += 5.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
            else
            {
                uArmRot -= 5.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
            #endregion

            #region Lower arm
            if (lArmRot >= larmMax) { lArmDirection = false; }
            else if (lArmRot <= larmMin) { lArmDirection = true; }
            if (lArmDirection)
            {
                lArmRot += 3.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
            else
            {
                lArmRot -= 3.0f * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
            #endregion
        }

        private void AnimateLegs(GameTime gameTime, float animationSpeed)
        {
            if (legRot >= legMax) { legDirection = false; }
            else if (legRot <= legMin) { legDirection = true; }

            //up
            if (legDirection)
            {
                legRot += animationSpeed * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
            else
            {
                legRot -= animationSpeed * (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.015f;
            }
        }

        public override void Update(GameTime gameTime, Matrix worldMatrix)
        {
            //slowly return the rotation of the torso to it's original place
            if (torsoRot < 0) { torsoRot += (float)gameTime.ElapsedGameTime.TotalSeconds * 20f; }
            if (torsoRot > 0) { torsoRot -= (float)gameTime.ElapsedGameTime.TotalSeconds * 20f; }
            _model.Bones["Spine"].Transform = Matrix.CreateRotationX(MathHelper.ToRadians(torsoRot)) * origTransforms["Spine"];

            _model.Bones["L Clavicle"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(clavRot)) * origTransforms["L Clavicle"];
            _model.Bones["R Clavicle"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(-clavRot)) * origTransforms["R Clavicle"];

            _model.Bones["L UpperArm"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(uArmRot)) * origTransforms["L UpperArm"];
            _model.Bones["R UpperArm"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(-uArmRot)) * origTransforms["R UpperArm"];
            _model.Bones["L Forearm"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(-lArmRot)) * origTransforms["L Forearm"];
            _model.Bones["R Forearm"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(-lArmRot)) * origTransforms["R Forearm"];

            _model.Bones["L Thigh"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(legRot)) * origTransforms["L Thigh"];
            _model.Bones["L Calf"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(-legRot)) * origTransforms["L Calf"];
            _model.Bones["L Foot"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(legRot * 0.33f)) * origTransforms["L Foot"];

            _model.Bones["R Thigh"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(-legRot)) * origTransforms["R Thigh"];
            _model.Bones["R Calf"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(legRot)) * origTransforms["R Calf"];
            _model.Bones["R Foot"].Transform = Matrix.CreateRotationZ(MathHelper.ToRadians(-legRot * 0.33f)) * origTransforms["R Foot"];


           // this.Position = new Vector3(0.1f,1.03f,0f);
            this.Position = _wf.getSphereCoords(new Vector3(pos2d.X, pos2d.Y, 1));
        //    this.Position = new Vector3(this.Position.X,this.Position.Y,this.Position.Z);

            this._model.Root.Transform = Matrix.CreateScale(0.05f)
            * Matrix.CreateRotationZ(MathHelper.ToRadians(90))   
            * Matrix.CreateRotationX(MathHelper.ToRadians(-90))
            * Matrix.CreateTranslation(this.Position);

            this.unitVector = Vector2.Transform(unitVector, Quaternion.CreateFromAxisAngle(Vector3.Up, leftRightRotation));
            this._rotation = Quaternion.CreateFromAxisAngle(Vector3.Up, leftRightRotation);
            this.objWorld = Matrix.CreateFromQuaternion(_rotation);
        }

        public override void Draw(BasicEffect effect, Matrix world)
        {
            //Matrix[] transforms = new Matrix[this._model.Bones.Count];
            //this._model.CopyAbsoluteBoneTransformsTo(transforms);

            //foreach (ModelMesh mm in this._model.Meshes)
            //{
            //    foreach (BasicEffect e in mm.Effects)
            //    {
            //        e.EnableDefaultLighting();

            //        e.CurrentTechnique.Passes[0].Apply();
            //    }
            //}

            this._model.Draw(this.objWorld * world, effect.View, effect.Projection);
        }

        public override void InitializeVertices()
        {
        }

        public override void InitializeIndices()
        {
        }
    }
}
